#ifndef RSIM_EOMDATA_H
#define RSIM_EOMDATA_H

/*-------------------------------------------------------------------------- *
* File: EOMData.h                                                            *
* Authors: Kishor Bhalerao                                                   *
* Email : kishor8dm@gmail.com                                                *
* Contributors:                                                              *
* Email:                                                                     *
* copyright (c) 2010 Authors.                                                *
*                                                                            *
* This program is free software: you can redistribute it and/or modify it    *
* under the terms of the GNU General Public License as published by the Free *
* Software Foundation, either version 3 of the License, or any later version.*
*                                                                            *
* This program is distributed in the hope that it will be useful, but        *
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
* or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
* for more details. (http://www.gnu.org/licenses/)                           *
* -------------------------------------------------------------------------- */

#include "LinAlgDecl.h"

namespace RSIM{

class Transform;

class EOMData{
        public:
                EOMData();
                
                ~EOMData();
                
                /** clears memory and sets all pointer values to NULL */
                void clear();
                
                void initEOM(const double& Mass,const Mat3x3& Inertia, const Transform& T_BF);
                
                Mat6x6* wgetdMk1();
                Mat3x3* wgetJk1();
                Vect6* wgetAk1();
                Vect6* wgetFkc1();
                Vect6* wgetdFk1();
                
                const Mat6x6* getdMk1()const;
                const Mat3x3* getJk1()const;
                const Vect6* getAk1()const;
                const Vect6* getFkc1()const;
                const Vect6* getdFk1()const;

        private:
                /** Spatial mass matrix shifted to frame F */
                Mat6x6 *dMk1_;
                
                /** Jk1 = Inertia - mass * [rx][rx]
                Inertia and rx are both written in frame F */
                Mat3x3 *Jk1_;
                
                /** Spatial acceleration of frame F expressed in F*/
                Vect6 *Ak1_;
                
                /** Spatial constraint force acting on frame F of this body exprssed in F. 
                These values normally lie in the non-working constraint force subspace of F if there
                is no prescribed motion acting on the joint connecting this body to parent. If 
                we have a prescribed motion, then potentially, all 6 values of this vector could be
                nonzero. */
                Vect6 *Fkc1_;
                
                /** Equation of motion of this body expressed in F is dMk1_ * Ak1_ -dFk1_ = Constraint forces shifted to F.
                All quantities are expressed in frame F*/
                Vect6 *dFk1_;
                
                
                
}; // class EOMData

inline Mat6x6* EOMData::wgetdMk1(){return dMk1_;}
inline const Mat6x6* EOMData::getdMk1()const{return dMk1_;}

inline Mat3x3* EOMData::wgetJk1(){return Jk1_;}
inline const Mat3x3* EOMData::getJk1()const{return Jk1_;}

inline Vect6* EOMData::wgetAk1(){return Ak1_;}
inline const Vect6* EOMData::getAk1()const{return Ak1_;}

inline Vect6* EOMData::wgetFkc1(){return Fkc1_;}
inline const Vect6* EOMData::getFkc1()const{return Fkc1_;}

inline Vect6* EOMData::wgetdFk1(){return dFk1_;}
inline const Vect6* EOMData::getdFk1()const{return dFk1_;}

} // namespace RSIM

#endif